﻿using System;
using System.Collections;
using System.Linq;
using System.Text;
using System.Net.Sockets;
using System.Net;
//using ConsoleApplication1.JSON;
using System.Globalization;

namespace ConsoleApplication1
{
    class Program
    {
        // Displays sending with a connected socket
        // using the overload that takes a buffer.
        public static int SendReceiveTest1(Socket server)
        {
           // String c = "{\"receiver\"";s
            //String command = {\"/cmd_vel\",\"msg\":{\"linear\":{\"x\":0.1,\"y\":0,\"z\":.5},\"angular\":{\"x\":0,\"y\":0,\"z\":0.1}},\"type\":\"geometry_msgs/Twist\"}";
            //String command1 = "{\"receiver\":\'/cmd_vel\',\"msg\":{\'linear\':{\'x\':0.1,\'y\':0,\'z\':.5},\'angular\':{\'x\':0,\'y\':0,\'z\':0.1}},\"type\":\'geometry_msgs/Twist\'}";
            //String command2 = "{\"receiver\":\"/cmd_vel\",\"msg\":{\"linear\":{\"x\":0.5,\"y\":0,\"z\":0}, \"angular\":{\"x\":0,\"y\":0,\"z\":0.5}},\"type\":\"geometry_msgs/Twist\"}";
            //String command3 = "\'/cmd_vel\', \'geometry_msgs/Twist\', '{\"linear\":{\"x\":.5,\"y\":0,\"z\":0}, \"angular\":{\"x\":0,\"y\":0,\"z\":.5}'";
            String command4 = "{\"receiver\":\"/cmd_vel\",\"msg\":{\"linear\":{\"x\":2.0,\"y\":0.0,\"z\":0.0}, \"angular\":{\"x\":0.0,\"y\":0.0,\"z\":1.0}},\"type\":\"geometry_msgs/Twist\"}";
            String command5 = "raw\r\n\r\n";

            //****ENTER THE DIFFERENT COMMANDS HERE****
            //byte[] msg = Encoding.UTF8.GetBytes(command5+hexValue+command4);
            //byte[] msg1 = Encoding.UTF8.GetBytes(hexValue+command4+hexValue1);
            //Console.WriteLine("Below is the byte array");

            //Console.WriteLine(msg1);
            //byte[] part1 = Encoding.UTF8.GetBytes(p1);
            //byte[] part2 = Encoding.UTF8.GetBytes(p2);

            byte[] bytes = new byte[256];
            try
            {
                /*
                int i = server.Send(msg);
                Console.WriteLine("Sent {0} bytes.", i);

                i = server.Receive(bytes);
                Console.WriteLine("Shows what is reveiced from the server");
                Console.WriteLine(Encoding.UTF8.GetString(bytes));
                */
                //messgage sent twice
                int i = server.Send(Encoding.UTF8.GetBytes(command5));
                i += server.Send(new byte[] { 0 });
                i += server.Send(Encoding.UTF8.GetBytes(command4));
                i += server.Send(new byte[] { 255 });
                i += server.Send(new byte[] { 0 });
                i += server.Send(Encoding.UTF8.GetBytes(command4));
                i += server.Send(new byte[] { 255 });

                Console.WriteLine("Sent {0} bytes", i);
                //lets try to send something else
                //i = server.Send(msg);//was msg1
                //Console.WriteLine("Sent {0} bytes", i);

                //lets try to get antoher reply
                //i = server.Receive(bytes);
                //Console.WriteLine(Encoding.UTF8.GetString(bytes));
            }
            catch (SocketException e)
            {
                Console.WriteLine("{0} Error code: {1}.", e.Message, e.ErrorCode);
                return (e.ErrorCode);
            }
            Console.ReadLine();
            return 0;
        }
        static void Main(string[] args)
        {
            
            IPAddress host = IPAddress.Parse("192.168.0.119");
            IPEndPoint hostep = new IPEndPoint(host, 9090);
            Socket m_socListener = new Socket(AddressFamily.InterNetwork, 
                                               SocketType.Stream, ProtocolType.Tcp);
            try
            {
                Console.WriteLine("Im about to try to connect");
                m_socListener.Connect(hostep);
                Console.WriteLine("I HAVE CONNECTED");
            }
            catch (SocketException e)
            {
                Console.WriteLine("Problem connecting to host");
                Console.WriteLine(e.ToString());
                m_socListener.Close();
                Console.ReadLine();
                return;
            }

            SendReceiveTest1(m_socListener);

            m_socListener.Close();

            Console.ReadLine();
        }
    }
}
